function M = madist(mdl, X, op)
%MADIST Computes the Mahalanobis distances to the Gaussian center
%
% [ Syntax ]
%   - M = madist(mdl, X)
%   - M = madist(mdl, X, 'shared')
%
% [ Arguments ]
%   - mdl:      the model or an array of models
%   - X:        the sample matrix
%   - M:        the resulting matrix of distances
%
% [ Description ]
%   - M = madist(mdl, X) computes the distances from the samples in X
%     to the center of Gaussian model(s).
%
%     Suppose there are n samples, then X should be a matrix of size
%     d x n, with each column giving a sample. 
%
%     If mdl is a single model object, then in the output, M is a row
%     vector of size 1 x n. If mdl is an array of K models, then M is
%     a row vector of size K x n. 
%
%   - M = madist(mdl, X, 'shared'). If you know that all models in mdl
%     share the same covariance matrix, you can use this syntax to tell
%     the function, such that it can make use of this fact to accelerate
%     the computation.
%
% [ Remarks ]
%   - If mdl is an array of multiple models, then all models should be 
%     in the same cache level. 
%
% [ History ]
%   - Created by Dahua Lin, on Dec 23, 2007
%

%% parse and verify input

assert(isnumeric(X) && ndims(X) == 2, ...
    'sltoolbox:slcfullgauss:madist:invalidarg', ...
    'X should be a numeric matrix');

K = numel(mdl);
d = getdim(mdl(1));

assert(size(X,1) == d, ...
    'sltoolbox:slcfullgauss:madist:invalidarg', ...
    'The dimension of samples does not match that of the models.');


if K > 1
    assert(isequal(mdl.clevel), ...
        'sltoolbox:slcfullgauss:madist:invalidarg', ...
        'all models in mdl should be in the same cache level');
end

clevel = mdl(1).clevel;

if nargin >= 3
    assert(ischar(op) && strcmp(op, 'shared'), ...
        'sltoolbox:slcfullgauss:madist:invalidarg', ...
        'the 3rd argument should be a string "shared"');
    
    shared = true;
else
    shared = false;
end


%% skeleton

switch clevel
    case 3
        M = compute_cl3(mdl, X, shared);
    case 2
        M = compute_cl2(mdl, X, shared);
    case 1
        M = compute_cl1(mdl, X, shared);
    case 0
        M = compute_cl0(mdl, X, shared);
end


%% core computation routines

% Level 3

function M = compute_cl3(mdl, X, shared)

K = numel(mdl);

% ord-0 term
t0 = [mdl.mu_ism_mu]';

% ord-1 term
t1 = ((-2) * [mdl.ism_mu])' * X;

% ord-2 term
if K == 1 || shared
    t2 = sum(X .* (mdl(1).invsm * X), 1);
else
    t2 = zeros(K, size(X,2));
    for i = 1 : K
        t2(i, :) = sum(X .* (mdl(i).invsm * X), 1);
    end
end

% combine terms
M = combine_terms(t0, t1, t2, K, shared);


% Level 2

function M = compute_cl2(mdl, X, shared)

K = numel(mdl);

% ord-0 term
t0 = [mdl.mu_ism_mu]';

% ord-1 term
t1 = ((-2) * [mdl.ism_mu])' * X;

% ord-2 term
if K == 1 || shared
    t2 = sum(X .* (mdl(1).sigma \ X), 1);
else
    t2 = zeros(K, size(X,2));
    for i = 1 : K
        t2(i, :) = sum(X .* (mdl(i).sigma \ X), 1);
    end
end

% combine terms
M = combine_terms(t0, t1, t2, K, shared);


% Level 1

function M = compute_cl1(mdl, X, shared)

K = numel(mdl);
n = size(X,2);

% ord-0 term
t0 = [mdl.mu_ism_mu]';

if K == 1 || shared    
    Za = mdl(1).sigma \ [X, mdl.mu];
    ism_x = Za(:, 1:n);
    ism_mu = Za(:, n+1:end);
    
    % ord-1 term
    t1 = ((-2) * ism_mu)' * X;
    
    % ord-2 term
    t2 = sum(X .* ism_x, 1);
    
else
    t1 = zeros(K, n);
    t2 = zeros(K, n);
    
    for i = 1 : K
        Za = mdl(i).sigma \ [X, mdl(i).mu];
        ism_x = Za(:, 1:n);
        ism_mu = Za(:, end);
        
        % ord-1 term
        t1(i, :) = ((-2) * ism_mu)' * X;
        
        % ord-2 term
        t2(i, :) = sum(X .* ism_x, 1);
    end
end
       
% combine terms    
M = combine_terms(t0, t1, t2, K, shared);


% Level 0

function M = compute_cl0(mdl, X, shared)

K = numel(mdl);
n = size(X, 2);

if K == 1 || shared
    Za = mdl(1).sigma \ [X, mdl.mu];
    ism_x = Za(:, 1:n);
    ism_mu = Za(:, n+1:end);
    
    % ord-0 term
    t0 = sum([mdl.mu] .* ism_mu, 1)';
    
    % ord-1 term
    t1 = ((-2) * ism_mu)' * X;
    
    % ord-2 term
    t2 = sum(X .* ism_x, 1);
    
else
    t0 = zeros(K, 1);
    t1 = zeros(K, n);
    t2 = zeros(K, n);
    
    for i = 1 : K
        Za = mdl(i).sigma \ [X, mdl(i).mu];
        ism_x = Za(:, 1:n);
        ism_mu = Za(:, end);
        
        % ord-0 term
        t0(i) = mdl(i).mu' * ism_mu;
        
        % ord-1 term
        t1(i, :) = ((-2) * ism_mu)' * X;
        
        % ord-2 term
        t2(i, :) = sum(X .* ism_x, 1);
    end
    
end

% combine terms    
M = combine_terms(t0, t1, t2, K, shared);



%% supporting functions

function M = combine_terms(t0, t1, t2, K, shared)

if K == 1
    M = t1 + t2 + t0;
    
elseif shared
    M = bsxfun(@plus, t1, t2);
    M = bsxfun(@plus, M, t0);
    
else
    M = t1 + t2;
    M = bsxfun(@plus, M, t0);
    
end

